{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import os\n",
    "import sys\n",
    "\n",
    "# monkey patches visualization and provides helpers to load geometries\n",
    "sys.path.append('..')\n",
    "import open3d_tutorial as o3dtut\n",
    "# change to True if you want to interact with the visualization windows\n",
    "o3dtut.interactive = not \"CI\" in os.environ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# RGBD integration\n",
    "Open3D implements a scalable RGBD image integration algorithm. The algorithm is based on the technique presented in [\\[Curless1996\\]](../reference.html#curless1996) and [\\[Newcombe2011\\]](../reference.html#newcombe2011). In order to support large scenes, we use a hierarchical hashing structure introduced in [Integrater in ElasticReconstruction](https://github.com/qianyizh/ElasticReconstruction/tree/master/Integrate)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Read trajectory from .log file\n",
    "This tutorial uses the function `read_trajectory` to read a camera trajectory from a [.log file](http://redwood-data.org/indoor/fileformat.html). A sample `.log` file is as follows.\n",
    "\n",
    "```\n",
    "# odometry.log\n",
    "0   0   1\n",
    "1   0   0   2\n",
    "0   1   0   2\n",
    "0   0   1 -0.3\n",
    "0   0   0   1\n",
    "1   1   2\n",
    "0.999988  3.08668e-005  0.0049181  1.99962\n",
    "-8.84184e-005  0.999932  0.0117022  1.97704\n",
    "-0.0049174  -0.0117024  0.999919  -0.300486\n",
    "0  0  0  1\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class CameraPose:\n",
    "\n",
    "    def __init__(self, meta, mat):\n",
    "        self.metadata = meta\n",
    "        self.pose = mat\n",
    "\n",
    "    def __str__(self):\n",
    "        return 'Metadata : ' + ' '.join(map(str, self.metadata)) + '\\n' + \\\n",
    "            \"Pose : \" + \"\\n\" + np.array_str(self.pose)\n",
    "\n",
    "\n",
    "def read_trajectory(filename):\n",
    "    traj = []\n",
    "    with open(filename, 'r') as f:\n",
    "        metastr = f.readline()\n",
    "        while metastr:\n",
    "            metadata = list(map(int, metastr.split()))\n",
    "            mat = np.zeros(shape=(4, 4))\n",
    "            for i in range(4):\n",
    "                matstr = f.readline()\n",
    "                mat[i, :] = np.fromstring(matstr, dtype=float, sep=' \\t')\n",
    "            traj.append(CameraPose(metadata, mat))\n",
    "            metastr = f.readline()\n",
    "    return traj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "redwood_rgbd = o3d.data.SampleRedwoodRGBDImages()\n",
    "camera_poses = read_trajectory(redwood_rgbd.odometry_log_path)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## TSDF volume integration\n",
    "Open3D provides two types of TSDF volumes: `UniformTSDFVolume` and `ScalableTSDFVolume`. The latter is recommended since it uses a hierarchical structure and thus supports larger scenes.\n",
    "\n",
    "`ScalableTSDFVolume` has several parameters. `voxel_length = 4.0 / 512.0` means a single voxel size for TSDF volume is $\\frac{4.0\\mathrm{m}}{512.0} = 7.8125\\mathrm{mm}$. Lowering this value makes a high-resolution TSDF volume, but the integration result can be susceptible to depth noise. `sdf_trunc = 0.04` specifies the truncation value for the signed distance function (SDF). When `color_type = TSDFVolumeColorType.RGB8`, 8 bit RGB color is also integrated as part of the TSDF volume. Float type intensity can be integrated with `color_type = TSDFVolumeColorType.Gray32` and `convert_rgb_to_intensity = True`. The color integration is inspired by [PCL](http://pointclouds.org/)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "volume = o3d.pipelines.integration.ScalableTSDFVolume(\n",
    "    voxel_length=4.0 / 512.0,\n",
    "    sdf_trunc=0.04,\n",
    "    color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)\n",
    "\n",
    "for i in range(len(camera_poses)):\n",
    "    print(\"Integrate {:d}-th image into the volume.\".format(i))\n",
    "    color = o3d.io.read_image(redwood_rgbd.color_paths[i])\n",
    "    depth = o3d.io.read_image(redwood_rgbd.depth_paths[i])\n",
    "    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(\n",
    "        color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False)\n",
    "    volume.integrate(\n",
    "        rgbd,\n",
    "        o3d.camera.PinholeCameraIntrinsic(\n",
    "            o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault),\n",
    "        np.linalg.inv(camera_poses[i].pose))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Extract a mesh\n",
    "Mesh extraction uses the marching cubes algorithm [\\[LorensenAndCline1987\\]](../reference.html#lorensenandcline1987)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Extract a triangle mesh from the volume and visualize it.\")\n",
    "mesh = volume.extract_triangle_mesh()\n",
    "mesh.compute_vertex_normals()\n",
    "o3d.visualization.draw_geometries([mesh],\n",
    "                                  front=[0.5297, -0.1873, -0.8272],\n",
    "                                  lookat=[2.0712, 2.0312, 1.7251],\n",
    "                                  up=[-0.0558, -0.9809, 0.1864],\n",
    "                                  zoom=0.47)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-info\">\n",
    "    \n",
    "**Note:**\n",
    "\n",
    "TSDF volume works like a weighted average filter in 3D space. If more frames are integrated, the volume produces a smoother and nicer mesh. Please check [Make fragments](../reconstruction_system/make_fragments.rst) for more examples.\n",
    "\n",
    "</div>"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Edit Metadata",
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
